#pragma config(Sensor, in1,    rightSensor,    sensorLineFollower)
#pragma config(Sensor, in2,    middleSensor,   sensorLineFollower)
#pragma config(Sensor, in3,    leftSensor,     sensorLineFollower)
#pragma config(Motor,  port2,           left,          tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port3,           right,         tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port4,           pingPongLaunch, tmotorServoContinuousRotation, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
/*
							 |	 |
							 _____
							|	\/	|
							 \___/
*/

void moveForward(int speed);
void moveBackward(int speed);
void driftLeft(int speed);
void driftRight(int speed);

//void followLine(int speed);

task main()
{
	const int TANK_MODE = 0; // Different modes
	const int ARCADE_MODE = 1;
	const int LINE_MODE = 2;
	//const int PING_PONG_MODE = 3;

	int thingyMode = TANK_MODE;
	int state = 0;
	int fullSpeed = -30;

	const int MOVE_FORWARD = 0; // Line commands
	const int MOVE_LEFT = 1;
	const int MOVE_RIGHT = 2;
	const int STOP = 3;

	/*bool tankMode = true;
	bool arcadeMode = false;
	bool lineMode = false;*/

	while(true)
	{
		//if(vexRT[Btn8L] == 1)
		//{
		//	thingyMode = TANK_MODE;
		//}
		//else if(vexRT[Btn8D] == 1)
		//{
		//	thingyMode = ARCADE_MODE;
		//}
		//else if(vexRT[Btn8R] == 1)
		//{
		//	thingyMode = LINE_MODE;
		//}
		//if(vexRT[Btn8U] == 1)
		//{
		//	thingyMode = PING_PONG_MODE;
		//}



		if(thingyMode == TANK_MODE)
		{
			motor[port3] = -vexRT[Ch2]; //Y-Axis for Right Joystick thingy
	  	motor[port2] = -vexRT[Ch3]; //Y-Axis for Left Joystick thingy
		}
		//else if(thingyMode == ARCADE_MODE)
		//{
	 // 	motor[port2] = vexRT[Ch3] / 2 - vexRT[Ch1] / 2;
	 // 	motor[port3] = vexRT[Ch3] / 2 + vexRT[Ch1] / 2;
		//}
		//else if(thingyMode == LINE_MODE) //followLine(fullSpeed);
		//{
		//	if(SensorValue[middleSensor] >= 250)
		//	{
		//		state = MOVE_FORWARD;
		//	}
		//	else if(SensorValue[leftSensor] >= 250)
		//	{
		//		state = MOVE_LEFT;
		//	}
		//	else if(SensorValue[rightSensor] >= 250)
		//	{
		//		state = MOVE_RIGHT;
		//	}
		//	else if (SensorValue[middleSensor] >= 250 && SensorValue[leftSensor] >= 250 && SensorValue[rightSensor] >= 250)
		//	{
		//		state = STOP;
		//	}

		//	switch(state)
		//	{
		//		case MOVE_FORWARD:
		//			moveForward(fullSpeed);
		//			break;

		//		case MOVE_LEFT:
		//			driftLeft(fullSpeed);
		//			break;

		//		case MOVE_RIGHT:
		//			driftRight(fullSpeed);
		//			break;

		//		case STOP:
		//			motor[port3] = 0;
		//			motor[port2] = 0;
		//			break;

		//		default:
		//		break;
		//	}

		//}
		//else if(thingyMode == PING_PONG_MODE)
		//{
		//	motor[port4] = vexRT[Ch3];
		//}
		else
		{
			// if for some reason we aren't in any mode, don't let motors move
			motor[port2] = 0;
			motor[port3] = 0;
			motor[port4] = 0;
		}

	}


}

void moveForward(int speed)
{
	motor[left] = speed ;
	motor[right] = speed;

}
void moveBackward(int speed)
{
	motor[left] = -speed;
	motor[right] = -speed;
}
void driftLeft(int speed)
{
	motor[left] = -speed;
	motor[right] = speed;
}
void driftRight(int speed)
{
	driftLeft(-speed);
}

/*void turnTankModeOn()
{
	if(Btn8L == 1)
		tankMode = true;



}*/

/*void followLine(int speed)
{
	int state = 0;

	const int MOVE_FORWARD = 0; // Line commands
	const int MOVE_LEFT = 1;
	const int MOVE_RIGHT = 2;

	if(thingyMode == LINE_MODE)
		{
				if(SensorValue[middleSensor] >= 250)
			{
				state = MOVE_FORWARD;
			}
			else if(SensorValue[leftSensor] >= 250)
			{
				state = MOVE_LEFT;
			}
			else if(SensorValue[rightSensor] >= 250)
			{
				state = MOVE_RIGHT;
			}

			switch(state)
			{
				case MOVE_FORWARD:
					moveForward(speed);
					break;

				case MOVE_LEFT:
					driftLeft(speed);
					break;

				case MOVE_RIGHT:
					driftRight(speed);
					break;

				default:
				break;
			}
		}
}*/
